#include<iostream>
#include<fstream>
#include<cstdlib>
#include "../LFOSC2Interface.h"
#include "../LFOSCommon.h"

using namespace std;

int main(int argc, char* argv[])
{
	LFOSC2Interface lci;
	unsigned char inBuffer[PIXELS];
	unsigned char buffer[4];
	unsigned char image[PIXELS];
	//bool* edgeImage;
	int nColors;
	DetectionInfo result;
	ifstream inFile;

	if(argc != 2)
	{
		cout << "Usage: EdgeDetectionTest <input image>\n";
		exit(1);
	}

	inFile.open(argv[1], ios::in | ios::binary);
	//inFile.open("edgeImage.bmp", ios::in | ios::binary);
	if(inFile.fail())
	{
		cout << argv[1] << " does not exist\n";
		exit(1);
	}

	inFile.seekg(46);
	inFile.read((char*)buffer, 4);
	nColors = buffer[0] | (buffer[1] << 8) |
			(buffer[2] << 16) | (buffer[3] << 24);

	inFile.seekg(54+4*nColors);

	inFile.read((char*)inBuffer, PIXELS);


	//image stored upside down - need to invert rows
	for(int y=0; y < ROWS; y++)
		for(int x=0; x < COLS; x++)
		{
			image[ROW_COL_TO_INDEX(x,ROWS-y-1)] = inBuffer[ROW_COL_TO_INDEX(x,y)];
		}

	result = lci.ProcessImage(image);
	if((result.type & Obstruction) != 0)
	{
		cout << "Obstruction Detected" << endl;
	}
	if((result.type & IntersectionReached) != 0)
	{
		cout << "Intersection Reached" << endl;
		cout << "Driveway: " << result.drivewayIntersection << endl;
		cout << "Left: " << result.roadLeft << endl;
		cout << "Right: " << result.roadRight << endl;
		cout << "Straight: " << result.roadStraight << endl;
		cout << "Intersection Location (" << result.intersection.x << ", " <<
				result.intersection.y << ")" << endl;;
	}
	if((result.type & IntersectionAhead) != 0)
	{
		cout << "IntersectionAhead" << endl;
		cout << "Driveway: " << result.drivewayIntersection << endl;
		cout << "Left: " << result.roadLeft << endl;
		cout << "Right: " << result.roadRight << endl;
		cout << "Straight: " << result.roadStraight << endl;
		cout << "Intersection Location (" << result.intersection.x << ", " <<
				result.intersection.y << ")" << endl;
	}
	if((result.type & LineDetection) != 0)
	{
		cout << "Closest Line: " << endl;
		cout << "From Center: " << result.fromCenter << endl;
		cout << "Angle from Center: " << result.angle << endl;
	}

	inFile.close();

	cout << "LFOS-C2 Interface Test Complete" << endl;

	return 0;
}
